/*    

    bobbietalk - a visual programming environment for toy robotics

    Copyright (C) 2013  Roberto Tiella

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
package bobbietalk.simulator.sensors;

import bobbietalk.simulator.world.Wall;
import bobbietalk.simulator.world.World;
import bobbietalk.simulator.world.WorldObject;
import java.awt.Shape;
import javax.vecmath.Vector3d;

/**
 *
 * @author roberto
 */
public class UltrasonicSensor extends AbstractSensor {

    private World world;
    private WorldObject body = new WorldObject();

    public void setWorld(World world) {
        this.world = world;
    }

    public Vector3d getPosition() {
        return body.getPosition();
    }

    public void setPosition(Vector3d position) {
        body.setPosition(position);
    }

    public Vector3d getOrientation() {
        return body.getOrientation();
    }

    public void setOrientation(Vector3d orientation) {
        body.setOrientation(orientation);
    }

    public double readDistance() {

        double x0 = getPosition().x;
        double y0 = getPosition().y;

        Vector3d v = new Vector3d();
        v.normalize(getOrientation());

        Wall w = world.getWall();

        Shape s = w.getShape();

        double d;

        for (d = 0.05; d < 1.0; d += 0.025) {

            if (s.contains(x0 + v.x * d, y0 + v.y * d)) {
                break;
            }

        }

        if (d >= 1) {
            d = Double.POSITIVE_INFINITY;
        }

        return d;

    }

    @Override
    public int getRawValue() {

        double d = readDistance();

        if (d == Double.POSITIVE_INFINITY) {
            return 255;
        } else {
            return (int) (d * 100);
        }

    }

    @Override
    public void setVisibility(boolean state) {
        throw new UnsupportedOperationException("Not supported yet."); //To change body of generated methods, choose Tools | Templates.
    }
}
